#简化时间
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import time
import hmac
import hashlib
import sys
from datetime import datetime, timedelta
import threading

import paho.mqtt.client as mqtt

import rclpy
from rclpy.node import Node

# 按你的包名与消息名导入（确保已用 ROS2 的 .msg 生成）
from my_custom_msgs.msg import TrajectoryInfo, TimeInfo


class MQTTPublisher(Node):
    def __init__(self):
        super().__init__('mqtt_publisher')

        # ---- 状态变量 ----
        self.current_trajectory = 0
        self.total_executions = 0
        self.current_time_str = ""
        self.elapsed_time_str = ""
        self.last_time_info_at = None  # 时间信息最近更新时间
        self.lock = threading.Lock()

        # ---- 订阅 ----
        self.create_subscription(
            TrajectoryInfo, 'trajectory_info', self.trajectory_info_callback, 10
        )
        self.create_subscription(
            TimeInfo, 'time_info', self.time_info_callback, 10
        )

        # ---- 阿里云 MQTT 配置（保持与你原代码一致）----
        self.broker = "k1dzhervpNg.iot-as-mqtt.cn-shanghai.aliyuncs.com"
        self.port = 1883
        self.topic = "/sys/k1dzhervpNg/Device1/thing/event/property/post"
        self.product_key = "k1dzhervpNg"
        self.device_name = "Device1"
        self.device_secret = "8ae2b4fe8c19a22f933c5900b2701a29"

        # 生成 client_id / username / password
        self.timestamp_ms = int(time.time() * 1000)
        self.client_id = f"{self.device_name}|securemode=3,signmethod=hmacsha1,timestamp={self.timestamp_ms}|"
        self.username = f"{self.device_name}&{self.product_key}"
        password_str = (
            f"clientId{self.device_name}"
            f"deviceName{self.device_name}"
            f"productKey{self.product_key}"
            f"timestamp{self.timestamp_ms}"
        )
        self.password = hmac.new(
            self.device_secret.encode(), password_str.encode(), hashlib.sha1
        ).hexdigest()

        # ---- 创建 MQTT 客户端 ----
        self.mqtt_client = mqtt.Client(self.client_id)
        self.mqtt_client.username_pw_set(self.username, self.password)

        # 绑定回调
        self.mqtt_client.on_connect = self.on_connect
        # 如需订阅下行消息，解除下一行注释
        # self.mqtt_client.on_message = self.on_message
        self.mqtt_client.on_log = self.on_log

        # 连接并启动网络循环线程
        self.mqtt_client.connect(self.broker, self.port, keepalive=60)
        self.mqtt_client.loop_start()

        # ---- 发布定时器：每 3 秒发一次 ----
        self.timer = self.create_timer(3.0, self.publish_tick)

        self.get_logger().info("MQTTPublisher node started (ROS2).")

    # ----------------- ROS2 回调 -----------------
    def trajectory_info_callback(self, msg: TrajectoryInfo):
        with self.lock:
            self.current_trajectory = int(msg.current_trajectory)
            self.total_executions = int(msg.total_executions)

    def time_info_callback(self, msg: TimeInfo):
        with self.lock:
            # 与原逻辑保持一致，存字符串
            self.current_time_str = str(msg.current_time)
            self.elapsed_time_str = str(msg.elapsed_time)
            self.last_time_info_at = datetime.now()

    # ----------------- MQTT 回调 -----------------
    def on_connect(self, client, userdata, flags, rc):
        if rc == 0:
            self.get_logger().info("MQTT 连接成功")
            client.subscribe(self.topic)
        else:
            self.get_logger().error(f"MQTT 连接失败，返回码: {rc}")

    def on_message(self, client, userdata, msg):
        self.get_logger().info(f"收到消息: {msg.topic} {msg.payload}")

    def on_log(self, client, userdata, level, buf):
        # 仅打印错误/警告，避免刷屏
        if level == mqtt.MQTT_LOG_ERR:
            self.get_logger().error(buf)
        elif level == mqtt.MQTT_LOG_WARNING:
            self.get_logger().warn(buf)
        # 如需更多日志：
        # elif level == mqtt.MQTT_LOG_INFO:
        #     self.get_logger().info(buf)
        # elif level == mqtt.MQTT_LOG_NOTICE:
        #     self.get_logger().info(buf)

    # ----------------- 发布逻辑 -----------------
    def publish_tick(self):
        """每 3 秒被计时器调用一次，发布一次数据。"""
        try:
            with self.lock:
                current_trajectory = self.current_trajectory
                total_executions = self.total_executions
                current_time = self.current_time_str
                elapsed_time = self.elapsed_time_str
                last_time_info_at = self.last_time_info_at

            # 判断是否“在线”：最近 2 秒是否收到 time_info
            online = (
                last_time_info_at is not None
                and (datetime.now() - last_time_info_at) <= timedelta(seconds=2)
                and isinstance(current_time, str) and isinstance(elapsed_time, str)
                and len(current_time) > 0 and len(elapsed_time) > 0
            )

            if online:
                # 解析 current_time: "YYYYMMDDHHMMSS"
                try:
                    current_time_dt = datetime.strptime(current_time, "%Y%m%d%H%M%S")
                    year = current_time_dt.year % 100  # 两位年份
                    month = current_time_dt.month
                    day = current_time_dt.day
                    hour = current_time_dt.hour
                    minute = current_time_dt.minute
                except Exception as e:
                    self.get_logger().warn(f"current_time 解析失败：{current_time} ({e})，发送默认数据。")
                    online = False

            if online:
                # 解析 elapsed_time: "HH:MM:SS"
                try:
                    elapsed_time_dt = datetime.strptime(elapsed_time, "%H:%M:%S")
                    hours = elapsed_time_dt.hour
                    minutes = elapsed_time_dt.minute
                    seconds = elapsed_time_dt.second
                except Exception as e:
                    self.get_logger().warn(f"elapsed_time 解析失败：{elapsed_time} ({e})，发送默认数据。")
                    online = False

            if online:
                # 创建数据字节数组（不包括 header）
                data = bytearray([
                    current_trajectory & 0xFF,    # 当前轨迹
                    total_executions & 0xFF,      # 执行次数
                    year, month, day, hour, minute,
                    hours & 0xFF, minutes & 0xFF, seconds & 0xFF,
                    0xcd, 0xdc
                ])

                header_hex = "0g02"              # 按你的协议要求保留该明文头
                data_hex = data.hex()
                full_message = header_hex + data_hex
            else:
                # 默认数据（离线）
                data = bytearray([
                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                    0x00, 0x00, 0x00, 0x00, 0x00,
                    0xcd, 0xdc
                ])
                header_hex = "0g02"
                data_hex = data.hex()
                full_message = header_hex + data_hex
                self.get_logger().warn("设备离线……发送默认数据")

            # 发布
            self.mqtt_client.publish(self.topic, payload=full_message, qos=0, retain=False)
            self.get_logger().info(f"已发送数据: {full_message}")

        except Exception as e:
            self.get_logger().error(f"发布异常：{e}")

    # ----------------- 关闭清理 -----------------
    def cleanup(self):
        try:
            self.mqtt_client.loop_stop()
            self.mqtt_client.disconnect()
        except Exception:
            pass


def main():
    rclpy.init()
    node = MQTTPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("收到中断信号，正在退出...")
    finally:
        node.cleanup()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
